#include <util.h>

#include <tiros/tiros.h>


#define STKSIZE  (TIROS_MIN_CTXT_SZ + 64)
#define NUM_HIGH_PRIO_TASKS  4
#define NUM_TASKS	     (NUM_HIGH_PRIO_TASKS +1)

/* An array of counting semaphores */
csem_t cs[NUM_HIGH_PRIO_TASKS];
mutex_t common_mutex;


/* Stacks for each of our tasks */
osword_t task_stack[NUM_TASKS][STKSIZE];



void tst_wait(void)
{
	delayms(1000);
}

void print_index(void)
{

	int i;
	putstring("R\tRunning - HP\n");
	putstring("P\tAchieved a P() on CS - HP\n");
	putstring("L\tLocked mutex - HP\n");
	putstring("U\tUnlocked mutex - HP\n");
	putstring("r\tRunning - LP\n");
	putstring("l\tLocked mutex - LP\n");
	putstring("u\tUnlocked mutex - LP\n");
	putstring("v\tDid a V() on CS - LP\n");
	putstring("0-3\t Before and after V on CS - LP\n\n");
	putstring("Task\tPrio\tElev\tMsg\n");
	for(i=50;i;i--)
		putchar('-');
	putchar('\n');

	
}

void printstatus(tid_t tid, tid_t bp, tid_t ep, unsigned char ch)
{

		putchar('T');
		puthexchar(tid);
		putchar('\t');
		puthexchar(bp);
		putchar('\t');
		puthexchar(ep);
		putchar('\t');
		putchar(ch);
		putchar('\n');

}


void low_prio_task(void * dummy)
{
	tid_t mytid;
	tid_t baseprio;
	tid_t elevprio;
	tid_t high_prio_task;
	int16_t rc, i;
	mytid = os_self_tid();
	baseprio = os_prio_get(mytid, 0);
	elevprio = os_prio_get(mytid, O_EFFECTIVE_PRIO);
	if (baseprio == ILLEGAL_ELEM || elevprio == ILLEGAL_ELEM)
		putstring("HP prio wrong");


	while (1) {
		tst_wait();
		/* Lock the mutex */
		printstatus(mytid, baseprio, elevprio, 'r');
		rc = mutex_lock(&common_mutex, 0, 0);
		printstatus(mytid, baseprio, elevprio, 'l');

		if (rc != 0)
			putstring("LP lock error");

		for (i=0; i< NUM_HIGH_PRIO_TASKS; i++) {
			high_prio_task = NUM_HIGH_PRIO_TASKS-1-i;
			printstatus(mytid, baseprio, elevprio, '0' +
				    high_prio_task); 
			tst_wait();

			/* Signal ready to them, the lowest prio task
			   first. This may cause this task to be switched
			   out. On continuing, our priority should be
			   elevated. */ 

			rc = csem_V(&cs[high_prio_task ]);
			elevprio = os_prio_get(mytid, O_EFFECTIVE_PRIO);			
			printstatus(mytid, baseprio, elevprio, 'v'); 
			if (rc < 0)
				putstring("LP V error");

		}

		rc = mutex_unlock(&common_mutex);
		if (rc != 0)
			putstring("LP unlock error");

		elevprio = os_prio_get(mytid, O_EFFECTIVE_PRIO);
		printstatus(mytid, baseprio, elevprio, 'u');

	}

}


void high_prio_task(void * dummy)
{
	tid_t mytid;
	tid_t baseprio;
	tid_t elevprio;
	int16_t rc;
	mytid = os_self_tid();
	baseprio = os_prio_get(mytid, 0);
	elevprio = os_prio_get(mytid, O_EFFECTIVE_PRIO);
	if (baseprio == ILLEGAL_ELEM || elevprio == ILLEGAL_ELEM)
		putstring("HP prio wrong");

	while(1) {
		printstatus(mytid, baseprio, elevprio, 'R');

		/* Try to get the counting semaphore.  Use baseprio as an
		   identifier since mytid is not guaranteed to
		   correspond to our    priority */
		rc = csem_P(&cs[baseprio], 0, 0);
		printstatus(mytid, baseprio, elevprio, 'P');

		if (rc < 0)
			putstring("HP P error");
		/* We have been signalled to wake.  Attempt a mutex lock. */
		rc = mutex_lock(&common_mutex, 0, 0);
		if (rc != 0)
			putstring("HP lock error");


		/* Acquired lock */
		elevprio = os_prio_get(mytid, O_EFFECTIVE_PRIO);
		printstatus(mytid, baseprio, elevprio, 'L');
		tst_wait();
		rc = mutex_unlock(&common_mutex);
		printstatus(mytid, baseprio, elevprio, 'U');
		if (rc != 0)
			putstring("HP unlock error");

	}
}



void platform_init(void);
int main(void) 
{
	int i;
	tid_t tid;
	platform_init();

	// --------------------------------------------------
	os_init();
	mutex_init(&common_mutex, ILLEGAL_ELEM);

	for (i=0; i< NUM_HIGH_PRIO_TASKS; i++) {
		csem_init(&cs[i], 0, TIROS_CSEMVAL_MAX);
		tid = os_task_create(high_prio_task, (osptr_t)i,
				      task_stack[i], STKSIZE,
				     i); 
		if (tid == ILLEGAL_ELEM)
			putstring("Task create failed");
	}



	/* Create low priority task */
	tid = os_task_create(low_prio_task, (osptr_t)i, 
			     task_stack[i], STKSIZE,
			     i);

	if (tid == ILLEGAL_ELEM)
		putstring("Task create failed");

	print_index();
	os_start();
	putstring("Started OS: Shouldn't reach here\n");
	while (1);
}

